/**
  ******************************************************************************
  * @file           : pid.c
  * @author         : Mark Xiang
  * @brief          : PID controller with output limit & output increment limit.
  ******************************************************************************
  * @attention
  *             1. This file has dependency on the definition of MAX(a,b) and MIN(a,b)
  * 
  ******************************************************************************
  */

/* ------------------------------ Includes ------------------------------ */
#include "pid.h"
#include "string.h"
/* ------------------------------ Defines ------------------------------ */
#define MAX(a, b) (a > b ? a : b)
#define MIN(a, b) (a < b ? a : b)
/* ------------------------------ Private Function Declarations ------------------------------ */

/* ------------------------------ Public Function Declarations ------------------------------ */
static void Init(Pid_t *pid, float kp, float ki, float kd, float output_limit, float output_increment_limit);
static void ResetKp(Pid_t *pid, float kp);
static void ResetKi(Pid_t *pid, float ki);
static void ResetKd(Pid_t *pid, float kd);
static void SetTarget(Pid_t *pid, float target);
static float DoPidCalc(Pid_t *pid, float feedback);
/* ------------------------------ Variable Declarations ------------------------------ */

/* ------------------------------ Private ------------------------------ */

/* ------------------------------ Public ------------------------------ */
/**
  * @brief  Initiate pid struct.
  * @param  pid pointer to pid struct
  * @param  kp kp
  * @param  ki ki
  * @param  kd kd
  * @param  output_limit limit of output of pid controller, no limit if set as 0
  * @param  output_increment_limit limit of output increment of pid controller, no limit if set as 0
  * @retval None
  */
static void Init(Pid_t *pid, float kp, float ki, float kd, float output_limit, float output_increment_limit)
{
  /* init variables as 0 */
  memset(pid, 0, sizeof(*pid));

  pid->kp = kp;
  pid->ki = ki;
  pid->kd = kd;
  pid->A = kp + ki + kd;
  pid->B = kp + 2 * kd;
  pid->C = kd;
  pid->output_limit = output_limit;
  pid->output_increment_limit = output_increment_limit;
}

/**
  * @brief  Change kp.
  * @param  pid pointer to pid struct
  * @param  kp new kp
  * @retval None
  */
static void ResetKp(Pid_t *pid, float kp)
{
  pid->kp = kp;
  pid->A = pid->kp + pid->ki + pid->kd;
  pid->B = pid->kp + 2 * pid->kd;
  pid->C = pid->kd;
}

/**
  * @brief  Change ki.
  * @param  pid pointer to pid struct
  * @param  ki new ki
  * @retval None
  */
static void ResetKi(Pid_t *pid, float ki)
{
  pid->ki = ki;
  pid->A = pid->kp + pid->ki + pid->kd;
  pid->B = pid->kp + 2 * pid->kd;
  pid->C = pid->kd;
}

/**
  * @brief  Change kd.
  * @param  pid pointer to pid struct
  * @param  kd new kd
  * @retval None
  */
static void ResetKd(Pid_t *pid, float kd)
{
  pid->kd = kd;
  pid->A = pid->kp + pid->ki + pid->kd;
  pid->B = pid->kp + 2 * pid->kd;
  pid->C = pid->kd;
}

/**
  * @brief  Set target of pid controller.
  * @param  pid pointer to pid struct
  * @param  target target of pid controller
  * @retval None
  */
static void SetTarget(Pid_t *pid, float target)
{
  pid->target = target;
}

/**
  * @brief  Do pid calculation.
  * @param  pid pointer to pid struct
  * @param  feedback feedback to pid controller
  * @retval output of pid controller
  */
static float DoPidCalc(Pid_t *pid, float feedback)
{
  pid->feedback = feedback;
  pid->err_last2 = pid->err_last1;
  pid->err_last1 = pid->err;
  pid->err = pid->target - pid->feedback;

  pid->output_increment = pid->A * pid->err - pid->B * pid->err_last1 + pid->C * pid->err_last2;
  if (pid->output_increment_limit > 1e-6f || pid->output_increment_limit < -1e-6f)
  {
    pid->output_increment = MIN(pid->output_increment, pid->output_increment_limit);
    pid->output_increment = MAX(pid->output_increment, -pid->output_increment_limit);
  }
  pid->output += pid->output_increment;
  if (pid->output_limit > 1e-6f || pid->output_limit < -1e-6f)
  {
    pid->output = MIN(pid->output, pid->output_limit);
    pid->output = MAX(pid->output, -pid->output_limit);
  }

  return pid->output;
}
/* ------------------------------ Manager Declaration ------------------------------ */
const PidManager_typedef Pid =
{
  Init,
  ResetKp,
  ResetKi,
  ResetKd,
  SetTarget,
  DoPidCalc
};

